let Kalman = require("./kalman");
let Mpu6050 = require("./mpu6050");
let ws = require("nodejs-websocket");

var mpu6050 = new Mpu6050();

// 偏移
let a_off_set_x = 0;
let g_off_set_y = 38;

//******角度参数************
let gyro_y = 0;        //Y轴陀螺仪数据暂存
let angle_gy  = 0;     //由角速度计算的倾斜角度
let accel_x  = 0;	     //X轴加速度值暂存
let angle_ax  = 0;     //由加速度计算的倾斜角度
let angle  = 0;        //小车最终倾斜角度

let mpu6050Data = {};

// 计算角度
function Angle_Calcu(){
  mpu6050Data = mpu6050.get_mpu6050Data();
  console.log('mpu6050Data = ',mpu6050Data);
  //------加速度--------------------------
	//范围为2g时，换算关系：16384 LSB/g
	//角度较小时，x=sinx得到角度（弧度）, deg = rad*180/3.14
  accel_x = mpu6050Data.accel_xyz.x;   //读取X轴加速度
  angle_ax = (accel_x - a_off_set_x)/16384;  //去除零点偏移,计算得到角度（弧度）
  angle_ax = angle_ax*180/3.14;     //弧度转换为度

  console.log('angle_ax = ',angle_ax);

  //-------角速度-------------------------
	//范围为2000deg/s时，换算关系：16.4 LSB/(deg/s)
  gyro_y = mpu6050Data.gyro_xyz.y;	  //读取Y轴 陀螺仪数据
  console.log('gyro_y = ',gyro_y);

  gyro_y = -(gyro_y + g_off_set_y)/16.4;         //去除零点偏移，计算角速度值,负号为方向处理 
  let Kalman_result = Kalman(angle,angle_ax,gyro_y);
  angle = Kalman_result.angle;
  gyro_y = Kalman_result.gyro_y;
  console.log('Kalman_result = ',Kalman_result);
  return {
    angle_ax: angle_ax,
    angle: Kalman_result.angle
  };

  // console.log('Kalman_result = ',Kalman_result);
}

let myInver = null;
var server = ws.createServer(function(conn){
  // console.log('server = ',server);
  // console.log('conn = ',conn);
  conn.on("text", function (str) {
    console.log("收到的信息为:"+str);
    sendStr(conn);
  })

  conn.on("close", function (code, reason) {
    console.log("关闭连接");
  });
  conn.on("error", function (code, reason) {
    console.log("异常关闭");
    if (myInver != null) {
      clearInterval(myInver);
      myInver =  null;
    } 
  });
}).listen(3000)
console.log("WebSocket建立完毕")


function sendStr(conn) {
  myInver = setInterval(function(){
    let Kalman_result = Angle_Calcu();
    conn.send(JSON.stringify(Kalman_result));
  },50);
}

